﻿using System.Collections.Generic;
using xn;
using System.Threading;

namespace KinectAPI
{   
    /// <summary>
    /// Represents the main class wich creates the skeleton body and
    /// tracks it
    /// </summary>
    public class BodyTracker : KinectAPIContext
    {

        #region Fields

        /// <summary>
        /// User skeleton
        /// </summary>
        private SkeletonCapability _skeletonCapbility;

        /// <summary>
        /// User pose
        /// </summary>
        private PoseDetectionCapability _poseDetectionCapability;

        /// <summary>
        /// Calibration pose
        /// </summary>
        private string _calibPose;

        /// <summary>
        /// 
        /// </summary>
        private Dictionary<uint, Dictionary<SkeletonJoint, SkeletonJointPosition>> _joints;

        private BodyGestureGenerator _gestureGenerator;

        #endregion

        #region Events

        /// <summary>
        /// Body event handler
        /// </summary>
        /// <param name="sender">The object firing the event.</param>
        /// <param name="e">Body Coordinates: nodes coordinates</param>
        public delegate void UsersUpdatedHandler(object sender, UsersEventArgs e);

        /// <summary>
        /// Body event
        /// </summary>
        public event UsersUpdatedHandler UsersUpdated;

        #endregion

        #region Constructors

        /// <summary>
        /// Creates a new instance of NuiSensor with the default configuration file.
        /// </summary>
        public BodyTracker()
            : this(XML_CONFIGURATION)
        {
        }

        /// <summary>
        /// Creates a new instance of NuiSensor with the specified configuration file.
        /// </summary>
        /// <param name="configuration">Configuration file path.</param>
        public BodyTracker(string configuration)
            : base(configuration)
        {
            _skeletonCapbility = new SkeletonCapability(_userGenerator);
            _poseDetectionCapability = new PoseDetectionCapability(_userGenerator);
            _calibPose = _skeletonCapbility.GetCalibrationPose();

            _userGenerator.NewUser += new UserGenerator.NewUserHandler(userGenerator_NewUser);
            _userGenerator.LostUser += new UserGenerator.LostUserHandler(userGenerator_LostUser);
            _poseDetectionCapability.PoseDetected += new PoseDetectionCapability.PoseDetectedHandler(poseDetectionCapability_PoseDetected);
            _skeletonCapbility.CalibrationEnd += new SkeletonCapability.CalibrationEndHandler(skeletonCapbility_CalibrationEnd);

            _skeletonCapbility.SetSkeletonProfile(SkeletonProfile.All);
            _joints = new Dictionary<uint, Dictionary<SkeletonJoint, SkeletonJointPosition>>();
            _userGenerator.StartGenerating();

            base.Camera += new CameraHandler(Camera_Updated);
        }

        #endregion

        #region Event Handler

        /// <summary>
        /// Creates the new user skeleton
        /// </summary>
        /// <param name="node">All the 15 nodes</param>
        /// <param name="id">The id body</param>
        /// <param name="success">The success or fail of the skeleton creation</param>
        void skeletonCapbility_CalibrationEnd(ProductionNode node, uint id, bool success)
        {
            if (success)
            {
                _skeletonCapbility.StartTracking(id);
                _joints.Add(id, new Dictionary<SkeletonJoint, SkeletonJointPosition>());
            }
            else
            {
                _poseDetectionCapability.StartPoseDetection(_calibPose, id);
            }
        }

        /// <summary>
        /// 
        /// </summary>
        /// <param name="node">All the 15 nodes</param>
        /// <param name="pose">The pose</param>
        /// <param name="id">The id body</param>
        void poseDetectionCapability_PoseDetected(ProductionNode node, string pose, uint id)
        {
            _poseDetectionCapability.StopPoseDetection(id);
            _skeletonCapbility.RequestCalibration(id, true);
        }

        /// <summary>
        /// Creates a skeleton of a new user
        /// </summary>
        /// <param name="node">All the 15 nodes</param>
        /// <param name="id">The id body</param>
        void userGenerator_NewUser(ProductionNode node, uint id)
        {
            _poseDetectionCapability.StartPoseDetection(_calibPose, id);
        }

        /// <summary>
        /// Removes a skeleton of a user no more in the Kinect area
        /// or no more detected
        /// </summary>
        /// <param name="node">All the 15 nodes</param>
        /// <param name="id">The id body</param>
        void userGenerator_LostUser(ProductionNode node, uint id)
        {
            _joints.Remove(id);
        }

        /// <summary>
        /// Starts the reading of the actions by the general thread
        /// </summary>
        /// <param name="sender"></param>
        void Camera_Updated(object sender)
        {
            ReaderThread();
        }

        #endregion

        #region Thread

        /// <summary>
        /// Represents actions done by the general thread
        /// </summary>
        private void ReaderThread()
        {
            uint[] users = _userGenerator.GetUsers();
            if (_joints.Count != 0)
            {
                foreach (uint user in users)
                {
                    GetJoint(user, SkeletonJoint.Head);
                    GetJoint(user, SkeletonJoint.Neck);

                    GetJoint(user, SkeletonJoint.LeftShoulder);
                    GetJoint(user, SkeletonJoint.LeftElbow);
                    GetJoint(user, SkeletonJoint.LeftHand);

                    GetJoint(user, SkeletonJoint.RightShoulder);
                    GetJoint(user, SkeletonJoint.RightElbow);
                    GetJoint(user, SkeletonJoint.RightHand);

                    GetJoint(user, SkeletonJoint.Torso);

                    GetJoint(user, SkeletonJoint.LeftHip);
                    GetJoint(user, SkeletonJoint.LeftKnee);
                    GetJoint(user, SkeletonJoint.LeftFoot);

                    GetJoint(user, SkeletonJoint.RightHip);
                    GetJoint(user, SkeletonJoint.RightKnee);
                    GetJoint(user, SkeletonJoint.RightFoot);
                }

                UsersUpdated(this, new UsersEventArgs(_joints));
            }
        }

        #endregion

        #region Private Methods

        /// <summary>
        /// Joins the 15 body nodes together
        /// </summary>
        /// <param name="user">The user</param>
        /// <param name="joint">Points representing the user skeleton</param>
        private void GetJoint(uint user, SkeletonJoint joint)
        {
            SkeletonJointOrientation jointOrient = new SkeletonJointOrientation();
            SkeletonJointPosition pos = new SkeletonJointPosition();
            _skeletonCapbility.GetSkeletonJointPosition(user, joint, ref pos);
            _skeletonCapbility.GetSkeletonJointOrientation(user, joint, jointOrient);

            if (pos.position.Z == 0)
            {
                pos.fConfidence = 0;
            }
            else
            {
                pos.position = _depthGenerator.ConvertRealWorldToProjective(pos.position);
            }
            if (_joints.ContainsKey(user))
            {
                _joints[user][joint] = pos;
            }
        }

        #endregion

    }
}
